/*
 * @Author: Mcfly
 * @Date: 2021-07-26 19:09:43
 * @LastEditTime: 2021-07-29 15:16:01
 * @LastEditors: Mcfly
 * @Description: 模拟人手的机器手，进行变量根据时间变化而变化的操作，就像人手推遥控器一样
 * @FilePath: \FTTfly\Ctrl\RoboHand.c
 * 本人github:https://github.com/McflyWZX
 */

#include "RoboHand.h"
#include "stdlib.h"

RoboHand *RoboHand_Init(float varStart, float varEnd, float K, bool allowFloat)
{
    RoboHand *roboHand = (RoboHand *)malloc(sizeof(RoboHand));
    roboHand->varStart = varStart;
    roboHand->varEnd = varEnd;
    roboHand->K = K;
    roboHand->deltaT = 1.0f / 200.0f;
    roboHand->var = varStart;
	roboHand->allowFloat = allowFloat;
	if(allowFloat && K < 0)
	{
		roboHand->K = -K;
	}
	return roboHand;
}

void RoboHand_UpdateFrame(RoboHand *roboHand)
{
	float K = roboHand->K;
	if(roboHand->allowFloat)
	{
		if(roboHand->var > roboHand->varEnd)
		{
			K = -K;
		}
	}
    float nextVar = roboHand->var + K * roboHand->deltaT;
    if (K > 0 && nextVar <= roboHand->varEnd)
    {
        roboHand->var = nextVar;
    } else if (K < 0 && nextVar >= roboHand->varEnd) {
        roboHand->var = nextVar;
    } else if(K > 0 && roboHand->var < roboHand->varEnd) {
        roboHand->var = roboHand->varEnd;
    } else if(K < 0 && roboHand->var > roboHand->varEnd) {
        roboHand->var = roboHand->varEnd;
    }
}

void RoboHand_SetVarStart(RoboHand *roboHand, float varStart)
{
	roboHand->varStart = varStart;
	roboHand->var = varStart;
}

void RoboHand_SetVarEnd(RoboHand *roboHand, float varEnd)
{
	roboHand->varEnd = varEnd;
}
